/*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.box2dflash.dynamics.contacts {
	import org.box2dflash.dynamics.contacts.Contact;
	import org.box2dflash.dynamics.Body;
	import org.box2dflash.common.math.Vec2;
	import org.box2dflash.collision.shapes.PolygonShape;
	import org.box2dflash.collision.Collision;
	import org.box2dflash.collision.shapes.*;
	import org.box2dflash.collision.*;
	import org.box2dflash.dynamics.*;
	import org.box2dflash.common.*;
	import org.box2dflash.common.math.*;
	use namespace Internal;

	public class PolygonContact extends Contact {
		static public function create(shape1:Shape, shape2:Shape/*NEVER USED , allocator:* */):Contact {
			//void* mem = allocator->Allocate(sizeof(PolyContact));
			return new PolygonContact(shape1, shape2);
		}

		static public function destroy(contact:Contact/*NEVER USED , allocator:* */):void {
		//((PolyContact*)contact)->~PolyContact();
		//allocator->Free(contact, sizeof(PolyContact));
		}

		public function PolygonContact(shape1:Shape, shape2:Shape):void {
			super(shape1, shape2);
			m_manifold = m_manifolds[0];
			//Settings.Assert(m_shape1.m_type == Shape.e_polygonShape);
			//Settings.Assert(m_shape2.m_type == Shape.e_polygonShape);
			m_manifold.pointCount = 0;
		}

		//~PolyContact() {}

		// store temp manifold to reduce calls to new
		private var m0:Manifold = new Manifold();
		static private const s_evalCP:ContactPoint = new ContactPoint();

		public override function evaluate(listener:ContactListener):void {
			var v1:Vec2;
			var v2:Vec2;
			var mp0:ManifoldPoint;
		
			var b1:Body = m_shape1.m_body;
			var b2:Body = m_shape2.m_body;
		
			var cp:ContactPoint;
			var i:int;
		
			//Manifold m0;
			//memcpy(&m0, &m_manifold, sizeof(Manifold));
			// TODO: make sure this is completely necessary
			m0 = m_manifold;
		
			Collision.collidePolygons(m_manifold, m_shape1 as PolygonShape, b1.m_xf, m_shape2 as PolygonShape, b2.m_xf);
			var persisted:Array = [false, false];
		
			cp = s_evalCP;
			cp.shape1 = m_shape1;
			cp.shape2 = m_shape2;
			cp.friction = m_friction;
			cp.restitution = m_restitution;
		
			// Match contact ids to facilitate warm starting.
			if (m_manifold.pointCount > 0) {
			
				// Match old contact ids to new contact ids and copy the
				// stored impulses to warm start the solver.
				for (i = 0;i < m_manifold.pointCount; ++i) {
					var mp:ManifoldPoint = m_manifold.points[ i ];
					mp.normalImpulse = 0.0;
					mp.tangentImpulse = 0.0;
					var found:Boolean = false;
					var idKey:uint = mp.id._key;
				
					for (var j:int = 0;j < m0.pointCount; ++j) {
						if (persisted[j] == true) {
							continue;
						}
					
						mp0 = m0.points[ j ];
					
						if (mp0.id._key == idKey) {
							persisted[j] = true;
							mp.normalImpulse = mp0.normalImpulse;
							mp.tangentImpulse = mp0.tangentImpulse;
	
							// A persistent point.
							found = true;
	
							// Report persistent point.
							if (listener != null) {
								cp.position = b1.worldPoint(mp.localPoint1);
								v1 = b1.linearVelocityFromLocalPoint(mp.localPoint1);
								v2 = b2.linearVelocityFromLocalPoint(mp.localPoint2);
								cp.velocity = new Vec2(v2.x - v1.x, v2.y - v1.y);
								cp.normal.v(m_manifold.normal);
								cp.separation = mp.separation;
								cp.id.key = idKey;
								listener.persist(cp);
							}
							break;
						}
					}
				
					// Report added point.
					if (found == false && listener != null) {
						cp.position = b1.worldPoint(mp.localPoint1);
						v1 = b1.linearVelocityFromLocalPoint(mp.localPoint1);
						v2 = b2.linearVelocityFromLocalPoint(mp.localPoint2);
						cp.velocity = new Vec2(v2.x - v1.x, v2.y - v1.y);
						cp.normal.v(m_manifold.normal);
						cp.separation = mp.separation;
						cp.id.key = idKey;
						listener.add(cp);
					}
				}
			
				m_manifoldCount = 1;
			}
		else {
				m_manifoldCount = 0;
			}
		
			if (listener == null) {
				return;
			}
		
			// Report removed points.
			for (i = 0;i < m0.pointCount; ++i) {
				if (persisted[i]) {
					continue;
				}
	
				mp0 = m0.points[ i ];
				cp.position = b1.worldPoint(mp0.localPoint1);
				v1 = b1.linearVelocityFromLocalPoint(mp0.localPoint1);
				v2 = b2.linearVelocityFromLocalPoint(mp0.localPoint2);
				cp.velocity = new Vec2(v2.x - v1.x, v2.y - v1.y);
				cp.normal.v(m0.normal);
				cp.separation = mp0.separation;
				cp.id.key = mp0.id._key;
				listener.remove(cp);
			}
		}

		public override function get manifolds():Array {
			return m_manifolds;
		}

		private var m_manifolds:Array = [new Manifold()];
		public var m_manifold:Manifold;
	}
}
